﻿using System;
using System.Collections.Generic;
//using System.Linq;
using System.Text;
using System.Windows;
using System.Diagnostics;
using System.IO;
using System.Xml;

using System.Xml.Serialization;

namespace AI
{

    public class Point
    {


        public int _index;

        public int Index { get { return _index; } set { _index = value; } }

        public int Range 
        {
            get{
                return _Range;
            }
            set
            {
                _Range = value;
            }
        }


        public bool CheckLight
        {
            get
            {
                return _bCheckLight;
            }
            set
            {
                _bCheckLight = value;
            }
        }
        private bool _bCheckLight = false;


        /// <summary>
        /// 도착지점 영역범위
        /// </summary>
        private int _Range;


        public int MyProperty { get; set; }


        //public Vector _P;

        //public Vector P
        //{
        //    get { return _P; }
        //    set { _P = value; }
        //}

        public Vector _GPSPos;

        public Vector GPSPos
        {
            get { return _GPSPos; }
            set { _GPSPos = value; }
        }

        public override string ToString()
        {
            return _index.ToString() + " : " + _GPSPos.ToString();
        }

        public bool bFixGPSPos { get {return _bFixGPSPos;} set{ _bFixGPSPos = value;} }
        bool _bFixGPSPos;


        //public Vector GetGPSPos()
        //{
        //    if (_bFixGPSPos)
        //        return GPSPos;
        //    return RateInfo.I.BMP2GPS(_P.X, _P.Y);
        //}
    }


    [XmlRoot("Node")]
    public class Node
    {
        public int _Index;
        public int _SIndex;
        public int _EIndex;

        public int Index { get { return _Index; } set { _Index = value; } }


        public int Weight
        {
            get { return _Weight; }
            set { _Weight = value; }
        }

        public int _Weight;

        public List<int> _NeighborList = new List<int>();

        public override string ToString()
        {
            return _SIndex.ToString() + " : " + _EIndex.ToString();
        }


    }

    public class NodeManager
    {
        public SerializableDictionary<int, Point> _PointList = new SerializableDictionary<int, Point>();

        //public Stack<int> _Path = null;
        // 키값을 바꿔야함.
        public SerializableDictionary<int, Node> _NodeList = new SerializableDictionary<int,Node>();

        MyDijkstra _Dijkstra = null;

        public Point GetPoint(double x, double y)
        {
            Vector p = new Vector(x, y);

            Point point = null;
            double Len = double.MaxValue;

            foreach (Point item in _PointList.Values)
            {
                double subLen = Vector.Subtract(item._GPSPos, p).Length;
                if (subLen < Len)
                {
                    point = item;
                    Len = subLen;
                }
            }

            return point;
        }

        public void RemovePoint(int key)
        {
            _PointList.Remove(key);
        }

        public Point AddPoint(double x, double y)
        {
            Point p = new Point();
            p._index = _PointList.Count;
            p._GPSPos.X = x;
            p._GPSPos.Y = y;

            _PointList.Add(p._index, p);

            return p;
        }

        public List<Node> RemoveNode(int StartIndex, int EndIndex)
        {
            List<Node> list = new List<Node>();

            // 현재는 
            foreach (Node item in _NodeList.Values)
            {

                if (item._SIndex == StartIndex && item._EIndex == EndIndex)
                    list.Add(item);

                if (item._SIndex == EndIndex && item._EIndex == StartIndex)
                    list.Add(item);
            }

            
            foreach (Node item in list)
            {
                _NodeList.Remove(item._Index);
            }


            // 현재는 
            foreach (Node item in _NodeList.Values)
            {
                
                if( item._NeighborList.Contains( list[0]._Index))
                {
                    item._NeighborList.Remove(list[0]._Index);
                }

                if (item._NeighborList.Contains(list[1]._Index))
                {
                    item._NeighborList.Remove(list[1]._Index);
                }

            }



            return list;
        }

        public Node GetNode(int StartIndex, int EndIndex)
        {
            foreach (Node item in _NodeList.Values)
            {

                if (item._SIndex == StartIndex && item._EIndex == EndIndex)
                    return item;

            }

            return null;

        }

        // 현재는 양방향 통행가능
        public List<Node> AddNode(int StartIndex, int EndIndex)
        {
            Node forwardNode = new Node();
            Node backwardNode = new Node();

            forwardNode._Index = _NodeList.Count;
            forwardNode._SIndex = StartIndex;
            forwardNode._EIndex = EndIndex;

            backwardNode._Index = _NodeList.Count+1;
            backwardNode._SIndex = EndIndex;
            backwardNode._EIndex = StartIndex;

            // 길이
            forwardNode._Weight = (int)Vector.Subtract(_PointList[StartIndex]._GPSPos, _PointList[EndIndex]._GPSPos).Length;
            backwardNode._Weight = forwardNode._Weight;
            // 현재는 
            foreach (Node item in _NodeList.Values)
            {

                // 유턴빼고 나머지 저장
                if (item._EIndex == forwardNode._SIndex && item._SIndex != forwardNode._EIndex)
                    item._NeighborList.Add(forwardNode._Index);

                if (forwardNode._EIndex == item._SIndex && forwardNode._SIndex != item._EIndex)
                    forwardNode._NeighborList.Add(item._Index);

                // 유턴빼고 나머지 저장
                if (item._EIndex == backwardNode._SIndex && item._SIndex != backwardNode._EIndex)
                    item._NeighborList.Add(backwardNode._Index);
                if (backwardNode._EIndex == item._SIndex && backwardNode._SIndex != item._EIndex)
                    backwardNode._NeighborList.Add(item._Index);

                //
            }

            _NodeList.Add(forwardNode._Index,forwardNode);
            _NodeList.Add(backwardNode._Index,backwardNode);

            List<Node> list = new List<Node>();
            list.Add(forwardNode);
            list.Add(backwardNode);

            return list;
        }


        ////  위치와 방향에 맞는 노드주기
        //public Node GetNode(double sx, double sy, Vector carDir)
        //{

        //    // 우선 가까운놈찾고

        //    Vector carPos = new Vector(sx, sy);


        //    Point ShortestPoint = GetPoint(sx, sy);
        //    Point ePoint = null;

        //    double minLen = double.MaxValue;

        //    foreach (Node node in _NodeList)
        //    {
        //        // 노드 앞뒤로 가장 가까운놈검사

        //        if (node._SIndex == ShortestPoint._index || node._EIndex == ShortestPoint._index)
        //        {
        //            if( 

        //            Point ep = _PointList[node._EIndex];
        //            double len = Vector.Subtract(s, ep._P).Length;
        //            if (len < minLen)
        //            {
        //                minLen = len;
        //                ePoint = ep;
        //            }
        //        }

        //    }

        //    List<Node> candidateNode = new List<Node>();



        //    foreach (Node node in _NodeList)
        //    {

        //        if (node._SIndex == sPoint._index && node._EIndex == ePoint._index)
        //        {
        //            candidateNode.Add(node);
        //        }

        //        if (node._EIndex == sPoint._index && node._SIndex == ePoint._index)
        //        {
        //            candidateNode.Add(node);
        //        }

        //    }

        //    double fDot = Vector.Multiply(curDir, GetNodeDir(candidateNode[0]));

        //    Node StartNode = null;

        //    if (0 <= fDot)
        //    {
        //        StartNode = candidateNode[0];
        //    }
        //    else
        //    {
        //        StartNode = candidateNode[1];
        //    }

        //    return StartNode;

        //}


        /// <summary>
        /// 제일 가까운놈을 리턴해준다
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <returns></returns>

        //http://www.cizi.pe.kr/Etc/PixelAndLine.htm

        //dir 단위백터로 생각함
        public WayInfo MakePath(Node StartNode, int eNodeIndex)
        {
            //Node StartNode = GetShortestNode(sx,sy,sdir);
            //Node EndNode = GetShortestNode(dx, dy);


            //Point DestPoint = GetPoint(dx, dy);


            int[,] cost = MakeCost();

            if (_Dijkstra == null)
            {
                _Dijkstra = new MyDijkstra(_NodeList.Count, cost);
            }
            else
            {
                //_Dijkstra.Init();
            }

            //List<int> EndNodeList = new List<int>();


            //foreach (Node node in _NodeList.Values)
            //{
            //    if( node._EIndex == eIndex)
            //        EndNodeList.Add( node._Index);
            //}

            WayInfo Path = _Dijkstra.GetPath(StartNode._Index, eNodeIndex);

            return Path;
        }

        int[,] MakeCost()
        {
            int nodeCount = _NodeList.Count;

            int[,] cost = new int[nodeCount, nodeCount];
            for (int i = 0; i < nodeCount; ++i)
            {
                for (int j = 0; j < nodeCount; ++j)
                {
                    cost[i, j] = int.MaxValue;
                }
            }

            foreach (Node node in _NodeList.Values)
            {

                foreach(int nodeIndex in node._NeighborList)
                {
                    cost[node._Index, nodeIndex] = node._Weight;
                }
                
            }



            return cost;

        }

        public Vector GetNodeDir(Node node)
        {
            return GetDir(_PointList[node._EIndex]._GPSPos.X - _PointList[node._SIndex]._GPSPos.X, _PointList[node._EIndex]._GPSPos.Y - _PointList[node._SIndex]._GPSPos.Y);
        }

        public Vector GetDir(double x, double y)
        {
            Vector dir = new Vector(x, y);
            double len = 1.0 / dir.Length;
            dir.X = len * dir.X;
            dir.Y = len * dir.Y;

            return dir;

        }

        double GetDir(Node node)
        {
            Vector v2 = new Vector(0, 1);
            Vector v1 = GetNodeDir(node);
            double len = 1.0 / v1.Length;
            v1.X = len * v1.X;
            v1.Y = len * v1.Y;

            double fDot = Vector.Multiply(v1, v2);

            double frad = Math.Acos(fDot);

            if (v1.X >= 0)
                return frad * (180 / Math.PI);


            return -(frad * (180 / Math.PI));
        }


        public Node GetShortestNode(double px, double py, Vector carDir)
        {
            double Shortest = double.MaxValue;
            Node ShortestNode = null;

            Vector P = new Vector(px, py);

            foreach (Node node in _NodeList.Values)
            {
                Vector S0 = _PointList[node._SIndex]._GPSPos;
                Vector S1 = _PointList[node._EIndex]._GPSPos;

                Vector nodeDir = GetDir(S1.X - S0.X, S1.Y - S0.Y);

                // 방향이 틀리면 버림
                if (Vector.Multiply(carDir, nodeDir) < 0)
                    continue;

                //점과 선의 거리 구하기 


                Vector v1 = P - S0;//new Vector(px-S0.X,py-S0.Y);
                Vector v2 = S1 - S0;//new Vector(px-S0.X,py-S0.Y);
                double v1v2 = Vector.Multiply(v1, v2);
                double v2v2 = Vector.Multiply(v2, v2);

                double value = v1v2 / v2v2;

                

                if (value < 0)
                {
                    continue;

                }
                else if (value < 1)
                {
                    double dist = Vector.Subtract(v1 , v2 * value).Length;

                    if (dist < Shortest)
                    {
                        Shortest = dist;
                        ShortestNode = node;
                    }
                    
                }
                else
                {
                    continue;
                }


            }

            return ShortestNode;
        }

        public Node GetShortestNode(double px, double py)
        {
            return null;
        }

        // 방문하지 않은 노드 중에서 가장 작은 비용의 노드를 선택



        public void Save()
        {

            XmlSerializer s = new XmlSerializer(_PointList.GetType());
            TextWriter w = new StreamWriter(@"robochamps/urban/AI/Point.xml");
            s.Serialize(w, _PointList);
            w.Close();

            s = new XmlSerializer(_NodeList.GetType());
            w = new StreamWriter(@"robochamps/urban/AI/Node.xml");
            s.Serialize(w, _NodeList);
            w.Close();
        }
        public void Load()
        {
            try
            {
                XmlSerializer s = new XmlSerializer(_PointList.GetType());
                using (TextReader r = new StreamReader(@"robochamps/urban/AI/Point.xml"))
                {
                    SerializableDictionary<int, Point> PointList = (SerializableDictionary<int, Point>)s.Deserialize(r);
                    if (PointList != null)
                        _PointList = PointList;

                }

                //foreach (Point p in _PointList.Values)
                //{
                //    p._GPSPos.X += 12200000;
                //    p._GPSPos.X /= 100000;

                //    p._GPSPos.Y += 4700000;
                //    p._GPSPos.Y /= 100000; 

                //}


                s = new XmlSerializer(_NodeList.GetType());

                using (TextReader r = new StreamReader(@"robochamps/urban/AI/Node.xml"))
                {
                    SerializableDictionary<int, Node> NodeList = (SerializableDictionary<int, Node>)s.Deserialize(r);

                    if (NodeList != null)
                        _NodeList = NodeList;

                }


            }
            catch
            {

            }

        }

    }
}
